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Miniature  Rotorcraft  Flight  Control  Stabilization  System 


by: 


Nathan  Siegers 

University  of  Alabama  in  Huntsville 
Huntsville,  Alabama 


ABSTRACT 

Attitude  determination  algorithms  for  a  small  rotorcraft  are  proposed  and  compared.  The  first  algorithm  is 
based  on  the  well  known  QUEST  algorithm  used  for  spacecraft  and  satellites.  Due  to  large  vibration  in  sensors  a 
pseudo-measurement  is  developed  from  gyroscope  measurements  and  rotational  kinematics.  The  pseudo¬ 
measurement  is  used  within  QUEST  to  smooth  attitude  estimations.  A  second  simple  gyro-compensated  tilt  sensor 
and  compass  is  proposed  and  compared  to  the  optimal  QUEST  solution.  Both  algorithms  are  shown  to  have  similar 
performance.  The  simple  gyro-compensated  tilt  sensor  and  compass  is  demonstrated  on  a  small  autonomous 
hovering  rotorcraft. 
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magnitude  of  flight  vector 

flight  distance  in  the  lateral  and  longitudinal  direction 

flight  position  derivatives  in  the  lateral  and  longitudinal  direction 

forgetting  factor 
Magnitude  of  earth’s  gravity 

gravity  vector  expressed  in  the  body  and  inertial  frames 

lateral  and  longitudinal  integral  error 

position  and  altitude  integral  error 

integral  error  of  position  vector  in  the  body  frame 

proportion-integral-derivative  gains 

longitudinal  and  lateral  cyclic  control  to  flap  gains 

main  rotor  hub  torsional  stiffness 

rotor  wake  intensity  factor 

longitudinal  point  of  helicopter  from  GPS  data 

lateral  point  of  helicopter  from  GPS  data 

earth’s  magnetic  vector  in  the  body,  M,  and  inertial  reference  frames 
optimum  tail  control 

maximum  limit  for  optimum  tail  saturation 
desired  longitudinal  GPS  position 
desired  lateral  GPS  position 

vehicle  angular  (roll,  pitch,  and  yaw)  rates  expressed  in  the  rotorcraft  body  reference  frame 
position  vector  in  the  body  frame 
velocity  vector  in  the  body  frame 

Transformation  matrix  from  the  inertial  reference  frame  to  the  rotorcraft  body  reference  frame 

Transformation  matrix  from  the  inertial  reference  frame  to  the  intermediate  M  reference  frame 

Transformation  matrix  from  the  intermediate  M  reference  frame  to  the  body  reference  frame 

bandwidth  of  tail  control 

longitudinal  and  lateral  cyclic  control  inputs 

main  rotor  collective  pitch  control  input 

position  vector  components  of  the  center  of  mass  expressed  in  the  inertial  reference  frame 

altitude  error  in  the  body  frame 

maximum  limit  for  altitude  saturation 

vertical  tail  offset  from  the  center  of  gravity  along  z  axis 

Angular  velocity  of  the  rotorcraft  with  respect  to  the  inertial  frame. 

Euler  roll,  pitch,  and  yaw  angles 
roll  angle  from  desired  position  vector 
roll  error  in  the  body  frame 
pitch  angle  from  desired  position  vector 
pitch  error  in  the  body  frame 
desired  yaw  orientation 


I.  INTRODUCTION 

Autonomous  rotorcraft  are  providing  improved  capability  in  performing  a  diverse  set  of  military  missions 
such  as  reconnaissance,  targeting,  border  patrol  and  environmental  sensing.  The  range  of  applications  envisioned 
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for  future  micro  rotorcraft  in  both  the  civilian  and  military  sectors  is  truly  staggering:  reconnaissance,  automated 
targeting,  sensor  emplacement,  monitoring,  surveying,  weapons  compliance,  hostage  release,  urban  maneuvering, 
battle  damage  assessment,  etc.  A  common  difficulty  in  applying  miniature  rotorcraft  to  these  areas  is  the  complexity 
and  specialization  of  the  control.  Inexpensive  commercial  autopilots  for  unmanned  vehicles  are  often  designed  for 
fixed  wing  aircraft  having  different  sensor  requirements  than  small  rotorcraft.  In  general,  rotorcraft  have  extreme 
vibration  that  make  miniature  inertial  measurement  difficult.  Larger  systems  can  filter  fast  dynamics  to  minimize  the 
impact  of  high  frequency  vibrations  and  noise;  however  miniature  rotorcraft  require  sensors  with  higher  frequency 
content  in  order  to  respond  to  their  fast  dynamics.  Typical  sensors  include  MEMS  accelerometers  which  are 
sensitive  to  vibration.  Inclusion  of  alternative  and  or  redundant  sensors  may  be  used  to  reduce  vibration  sensitivity 
and  add  useful  additional  feedback. 

The  estimation  of  orientation  using  multiple  vector  measurements  is  a  standard  attitude  determination 
problem  used  by  spacecraft  and  satellites.  This  problem  can  be  separated  into  two  types  of  solutions;  deterministic 
and  optimal  algorithms.  Deterministic  algorithms  use  two  measurement  vectors  (four  pieces  of  information)  and 
discard  part  of  the  measurements  so  that  the  orientation  parameters  can  be  found.  A  common  deterministic  algorithm 
is  the  TRIAD  algorithm  (TRI-axial  Attitude  Determination  system).  The  algorithm  was  originally  developed  by 
Black  [2]  with  no  name  given.  The  method  was  later  published  by  Lerner  [3]  under  the  name  “algebraic  method.” 
The  algorithm  then  appears  as  the  TRIAD  algorithm  in  [4].  The  TRIAD  algorithm  uses  two  measurements,  the  first 
assumed  to  be  more  reliable  than  the  second.  Two  right-handed  orthornomal  triads  are  formed  and  used  to  find  the 
desired  transformation  matrix.  Drawbacks  of  the  TRIAD  algorithm  are  that  it  accommodates  only  two  observations, 
and  some  accuracy  is  lost  because  part  of  the  second  measurement  is  discarded. 

Optimal  attitude  determination  algorithms  differ  from  deterministic  algorithms  by  computing  a  best 
estimate  which  minimizes  a  loss  function  J.  A  general  statement  of  the  optimal  attitude  determination  problem  was 
first  posed  by  Wahba  [5].  A  solution  was  later  posed  by  Wahba  et  al  [6].  However,  it  was  Davenport  and  Keat  (see 
ref  [4]  and  [7])  who  used  a  quaternion  representation  that  leads  to  an  eigenvalue  equation  solution  called  the  q- 
method.  Shuster  later  developed  an  efficient  approximate  method  to  the  q-method  called  QUEST  [4]  that  allows  the 
approximation  of  the  optimal  quaternion  without  solving  the  eigenvalue  problem. 

The  work  reported  here  evaluates  the  well  know  attitude  determination  algorithms  for  use  on  small  rotorcraft.  In 
order  to  reduce  noise  from  vibration  two  alternative  methods  are  proposed  and  evaluated.  The  first  alternative 
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algorithm  adds  into  QUEST  a  pseudo-measurement  vector  derived  from  rotational  kinematics.  As  a  comparison,  a 
simple  gyro-compensated  tilt  sensor  and  compass  is  also  developed.  Finally,  the  orientation  algorithms  are  tested  on 
a  small  autonomous  hovering  rotorcraft. 


II.  ROTORCRAFT  ORIENTATION 


The  orientation  of  a  rotorcraft  can  be  defined  by  the  standard  aerospace  sequence  of  body  fixed  rotations  using 
Euler  angles.  The  inertial  axis  has  the  Iraxis  horizontal  to  ground  directed  north,  the  Jraxis  also  horizontal  and 
pointing  east  and  K(  directly  down.  The  first  rotation  is  by  the  angle  \|/  about  the  K|  axis  resulting  in  a  M  frame, 
followed  by  a  second  rotation  0  about  the  JM  axis  resulting  in  a  N  frame.  Finally,  a  rotation  ip  about  the  IN  axis 
results  in  the  body  frame  B.  The  individual  transformations  for  each  rotation  are  provided  in  Eqs.  (1)  to  (3)  where 
the  common  shorthand  notation  for  trigonometric  functions  is  employed  where  sin  (or)  =  Sa  ,  cos(ct)  =  s  and 

tan(a)  =  ta . 
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The  complete  transformation  from  the  inertial  frame  I  to  the  body  frame  B  is  given  as  Tw  where, 

\^ib\  =mvim 


(4) 


4 


0 

~so 

T 

J MB 

= 

s?e 

c* 

Sfo 

_C*S9 

~st 

c<fe 

T  = 

1IB 


C<P¥ 


C(£y, 


WV_CVV  W-//  +cfw  sfo 
WV+VV  Wr%/  cfe 


(5) 
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Using  the  transformation  TIB  in  Eq.  (6)  the  components  of  a  vector  in  I  and  B  can  be  related.  Consider  specifically 
the  gravity  vector  g 
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If  earth’s  gravity  is  measured  in  B  it  is  straight  forward  to  solve  for  cp  and  0  using  Eq.  (8). 
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From  Eqs.  (5),  (9),  and  (10)  TMB  can  also  be  calculated.  Two  of  the  three  Euler  angles  have  been  found  from  one 

measurement  vector.  Since  gB  is  independent  of  ip  it  is  clear  we  have  no  information  about  the  third  Euler  angle.  In 
general,  each  vector  measurement  provides  only  two  pieces  of  information  and  the  third  is  constrained  by  the 
magnitude  since  an  orthonormal  transformation  preserves  magnitude.  To  find  the  final  Euler  angle  a  second 


5 


measurement  vector  is  required.  Consider  a  magnetometer  measuring  earth’s  magnetic  vector  m  where  it  is 


assumed  the  inertial  frame’s  I-axis  is  aligned  north  and  horizontal  to  the  earth’s  surface. 
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Since,  TMB  is  known  from  Eqs.  (5),  (9),  and  (10)  the  magnetic  vector  in  the  M  frame  can  be  found. 
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Comparing  Eq.  (13)  to  in  results  in  three  equations. 
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Using  only  the  second  equation  the  final  Euler  angle  \|/  is  found  as 


y/  atan2  (  Mym ,  Mxm  ) 


(15) 


Using  two  vector  measurements  ( gB  and  mB  )  the  three  Euler  angles  can  be  found.  More  specifically,  two  pieces  of 
information  were  used  to  find  (p  and  0  and  one  piece  was  used  to  find  \\i.  In  general,  at  least  two  measurement 
vectors  are  needed,  we  can  discard  one  piece  of  information  and  use  three  to  solve  for  the  three  unknowns  or  use  all 
four.  If  all  four  pieces  of  information  are  used,  the  problem  of  orientation  is  over-determined  and  a  least  squares 
solution  may  be  employed. 
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The  orientation  solution  presented  in  Eqs.  (9,10)  and  (15)  is  specific  to  the  Euler  angle  representation.  It  is 
possible  to  describe  the  orientation  using  alternative  formulations.  In  general,  we  can  relate  a  vector  in  two  frames 
(In  our  case  I  and  B)  using  Eq.  (16) 


Vkb=[Tab]Vka 


(16) 


where,  Vkb  is  an  arbitrary  k  vector  expressed  in  frame  “b”,  Vka  is  an  arbitrary  k  vector  expressed  in  frame  “a”,  and 
T  h  is  the  3x3  orthonormal  transformation  matrix  from  “a”  to  “b”.  The  orthornomal  transformation  matrix  can  be 
defined  using  any  valid  set  of  orientation  map.  Note,  in  Eq.  (6)  Euler  angles  were  used  to  describe/^  .  A  common 
alternative  to  Euler  angles  is  a  quaternion  representation.  Consider  a  quaternion  q  where, 
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It  is  well  known  that  the  transformation  using  a  quaternion  q  can  be  expressed  using  the  four  quaternion 
components  [1], 
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Since  Eq.  (19)  uses  four  quaternion  components  to  express  Tw  which  is  determined  by  only  three  parameters  the 
quaternion  is  constrained  to  have  unity  magnitude,  thus  having  only  three  unique  parameters. 


qq  =  1 


(20) 
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Any  of  the  three  representations  can  be  used  to  define  orientation  (  3  Euler  angles,  Quaternions,  or  Direction 
Cosines).  However,  as  shown  in  the  previous  example,  at  least  two  vector  measurements  are  required  to  solve  the 
problem. 

III.  ATTITUDE  DETERMINATION 


Using  Eq.  (16)  the  attitude  determination  problem  can  be  generalized  in  the  following  statement. 


Using  at  least  two  measurement  vectors  find  the  Euler  angles,  Quaternion,  or  Direction  Cosines 
that  define  the  orientation  matrix. 

This  problem  can  be  separated  into  two  types  of  solutions;  deterministic  and  optimal  algorithms. 
Deterministic  algorithms  use  two  measurement  vectors  (four  pieces  of  information),  discards  part  of  the 
measurements  so  that  the  orientation  parameters  can  be  found,  similar  to  the  Euler  angle  example.  The  TRIAD 
algorithm  uses  two  measurements  as  in  Eqs.  (22)  and  (23),  the  first  assumed  to  be  more  reliable  than  the  second.  . 
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Two  right-handed  orthornomal  triads  of  vectors  {hB,t2B,T3B}  and 
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^1/X^2/ 

h b  h b  x  ^2 B  ^3/  hi  X  ^21 


Using  the  two  triads,  the  solution  to  TIB  is  shown  in  Eq.  (25). 


(25) 


[Tib]  =  Vxb 


*2B 


*3  B 


R 


' '21 


The  solution  to  TIB  satisfies  Eq.  22  exactly  and  in  the  absence  of  noise  Eq.  (25)  also  satisfies  (23)  exactly.  In  the 

presence  of  noise  (23)  will  not  be  satisfied.  The  TRIAD  algorithm  is  limited  to  using  only  two  observations,  and 
some  accuracy  is  lost  because  part  of  the  second  measurement  is  discarded. 

Optimal  attitude  determination  algorithms  differ  from  deterministic  algorithms  by  computing  a  best 
estimate  minimizing  a  loss  function  J.  A  general  statement  of  the  optimal  attitude  determination  problem  was  first 
posed  by  Wahba  [5]  in  Eq.  (26)  where,  the  task  is  to  find  a  that  minimizes  J  using  the  N  measurements. 

1  N 

J  {BIB  )  =  T  ^  Wk  | ^IcB  ~  TmVki  |  (26) 

^  k=l 

Davenport  and  Keat  [4,7]  developed  the  optimal  q-method  using  a  quaternion  representation  leading  to  an 
eigenvalue  equation  solution.  The  q-method  [7]  rewrites  (26)  using  quaternions  leading  to  an  alternative  gain 
function. 

g(q)  =  i-^(^)  =  qr^q  (21) 


Where,  K  is  a  4  x  4  matrix  defined  in  Eq.  (28). 


K  = 


S  -<j  I 
ZT 


z 

a 


(28a) 


B  =  TjWk{VkBVkIT) 
k= 1 


s  =  b+bt 


(28b) 


(28c) 


Z  ~  [^23  B3 1  BU  B2\  ] 


(28d) 


cr  =  tr\B^ 


(28e) 


In  order  to  minimize  J,  Eq.  (27)  must  be  maximized  taking  into  the  unit  quaternion  constraint  in  Eq.  (20).  The 
constraint  in  (20)  can  be  accounted  for  by  appending  a  Lagrange  multiplier  to  (27)  resulting  in  a  new  gain  function. 
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g'(q)  =  qr^q-^qrq 


(29) 


Differentiating  Eq.  (29)  leads  to  a  stationary  point  when  Eq.  (30)  is  satisfied. 

Kq  =  Aq 


(30) 


Equation  30  is  recognized  as  an  eigenvalue  problem,  thus  the  optimal  quaternion  is  an  eigenvector  of  K.  Substitution 
of  (30)  into  (27)  yields 


g(q)  =  qTAq  =  AqTq  =  A 


(31) 


Or  g(q)  is  maximized  for  the  largest  eigenvalue  of  K  and  q  is  the  corresponding  eigenvector.  The  QUEST 
algorithm  developed  by  Shuster  [4]  approximates  the  optimal  q-method  solution  without  solving  the  eigenvalue 
problem.  QUEST  uses  the  approximation  in  Eq.  (32)  to  quickly  solve  for  q  . 

N 

4nax  ~Y,Wk  (32) 

k= 1 

Both  the  TRIAD  and  QUEST  algorithms  are  well  known  and  have  been  used  extensively  for  attitude 
determination  of  spacecraft  and  satellites.  These  systems  can  use  multiple  vector  measurements  from 
magnetometers,  sun  sensors,  star  sensors,  and  directional  antennas.  Theoretically,  both  methods  can  be  used  on 
small  unmanned  rotorcraft  and  fixed  wing  systems.  In  both  cases,  earth’s  gravity  and  magnetic  field  can  be  used  as 
the  two  measurement  vectors.  Practically,  accelerometers  on  unmanned  systems  measure  not  only  earth’s  gravity  but 
also  the  systems  acceleration  and  noise.  Often  significant  filtering  is  used  to  reduce  noise  and  transient  acceleration 
at  the  cost  of  sensor  delay.  In  moderate  dynamic  systems  this  approach  is  often  adequate  and  reasonable  estimates 
can  be  achieved. 

To  demonstrate  the  performance  of  both  the  TRIAD  algorithm  and  QUEST  on  an  example  small  rotorcraft 
a  set  of  data  was  generated  as  shown  in  Fig.  1.  Using  the  known  Euler  angles,  g1 ,  and  1)1 1  measurement  data  was 
generated  by  adding  Gaussian  noise  with  a  standard  deviation  of  0.30  G’s  onto  gB  and  a  standard  deviation  of  2% 
the  earth’s  magnetic  field  onto  mB  as  shown  in  Fig.  2. 
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Figure  1.  True  Orientation  and  Angular  Velocities 


Time  (s) 


Figure  2.  Measurements  including  noise 


Results  from  the  TRIAD  and  QUEST  algorithms  are  shown  in  Figs.  3  and  4  with  errors  in  Figs.  5  and  6. 
Both  the  acceleration  and  magnetometer  measurements  have  been  pre-filtered  with  a  first-order  filter  having  a  time 
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constant  of  0.125  seconds.  As  expected,  the  large  high  frequency  errors  from  the  poor  sensors  results  in  large  error 
of  near  10  deg  for  both  <p  and  0  and  20  deg  for  \|/.  Also  as  expected,  QUEST  performs  slightly  better  than  the  TRIAD 
algorithm  because  of  the  optimal  use  of  measurement  information.  However,  in  both  cases  the  estimates  are  poor. 
Improvements  could  be  made  by  filtering  the  data  more;  however,  the  increased  delay  in  estimation  becomes 
problematic. 


Figure  3.  TRIAD  Euler  angle  estimates 
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Time  (s) 

Figure  6.  QUEST  Euler  angle  errors 

IV.  IMPROVED  ATTITUDE  ALGORITHMS 

Small  unmanned  rotorcraft  provide  a  severe  dynamic  environment  for  attitude  estimation  due  to  their  fast 
dynamic  response  and  extreme  vibration.  Sources  of  vibration  include  the  main  rotor,  tail  rotor,  and  blade  flapping 
dynamics.  In  addition,  their  small  size  requires  light  and  small  sensors,  both  resulting  in  low  quality  measurements. 
The  additional  filtering  needed  to  eliminate  vibration  results  in  additional  delay  in  orientation  estimation.  However, 
due  to  the  fast  dynamics,  this  additional  delay  leads  to  possible  control  instability.  An  alternative  to  passive  filtering 
is  required  to  improve  orientation  estimation. 

Consider  two  sequential  orientation  estimates  using  gravity  vector  measurements  corrupted  by  vibration. 
From  one  estimate  to  the  next  the  estimated  orientation  may  change  significantly  over  a  short  period  of  time  as  seen 
in  both  Figs.  5  and  6.  If  the  orientation  did  indeed  change  significantly  it  would  require  a  large  angular  velocity.  In 
the  absence  of  any  other  information  it  is  not  possible  to  determine  if  the  change  in  gravity  measurement  is  from  a 
true  rotation,  acceleration  or  noise.  The  incorporation  of  a  three-axis  MEMs  gyro  can  be  used  to  measure  COB/I  the 
angular  velocity  of  the  rotorcraft  with  respect  to  an  inertial  reference  frame. 
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^B/I  pi B  B  1  H  n 


(33) 


The  kinematic  differential  equations  relating  the  change  in  orientation  with  C0B/I  for  Euler  angles.  Quaternions,  and 


Direction  Cosines  are  shown  in  Eqs.  (34-36)  and  can  be  used  to  quantify  expected  changes  in  orientation  over 


measurements  [1], 
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(34) 


(35) 


(36) 


A.  Psuedo-Measurement  Vector 

QUEST  has  the  ability  to  easily  incorporate  additional  vector  measurements.  However,  by  inspection  of 
Eqs.  (22)  and  (23)  the  angular  velocity  (0BjI  cannot  be  directly  added  as  a  third  vector  measurement  into  the  q- 

method  or  QUEST  because  G)BII  in  the  I  frame  is  unknown.  The  proposed  solution  is  to  create  a  third  pseudo¬ 
measurement  vector  from  directly  from  COBn  by  using  the  rotational  kinematic  differential  equations. 

The  first  step  is  to  select  a  pseudo-vector  V  ,  in  the  inertial  frame  I  with  the  only  restriction  being  it  cannot 
be  collinear  with  either  the  gravity  or  magnetic  vector.  Using  the  previous  estimate  of  q  along  with  (19)  the 
previous  pseudo-vector  in  B  is, 

V;  =  [^re]  V  (37' 
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At  the  next  measurement  Eq.  (35)  is  used  to  predict  qby  propagation  of  the  quaternion  kinematics  according  to  Eqs. 
(38,  39) 


q  =  q  +  A/q 


(38) 
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Equation  39  is  the  new  pseudo-measurement  vector  V  B  .  The  weights  (28b)  are  selected  to  adjust  the  amount  of 
smoothing  from  the  pseudo-vector.  If  the  weight  on  v  B  is  zero  the  solution  reduces  to  the  original  estimator.  As  the 


weights  on  the  true  measurements  approach  zero  the  estimator  reduces  to  numerical  integration  of  the  angular 
velocities,  which  would  quickly  diverge  due  to  biases  in  the  gyros.  The  pseudo-measurement  vector  and  true 
measurements  form  complimentary  filters.  Integration  of  the  angular  velocity  smoothes  the  measurements  without 
adding  delay,  the  other  measurements  eliminate  divergence  from  integration  of  the  gyros. 


B.  Gyro-Compensated  Tilt  Sensor  and  Compass 

A  drawback  of  the  previous  methods  is  in  the  magnetic  vector  in  the  inertial  frame  I  is  required.  Due  to 
magnetic  variation  and  declination  the  magnetic  vector  varies  as  a  function  of  latitude,  longitude,  altitude,  and  time. 
This  requires  an  initial  estimation  unlike  the  gravity  vector.  Unlike  the  TRIAD  and  QUEST  algorithms,  the 

deterministic  method  presented  in  Eqs.  (9,  10)  and  (15)  does  not  require  m'  but  only  uses  the  measurement  as  a 
compass.  As  with  the  previous  proposed  algorithm  the  angular  velocity  COB/I  can  be  used  to  form  complimentary 

filtering.  Notice  by  inspection  of  (6),  (8),  and  (21)  thatg^  is  a  direct  measurement  of  the  direction 

cosines.  Instead  of  estimating  (p  and  0  from  (8)  and  (9)  the  gyro-compensated  tilt  sensor  will  estimate  /13,/73,/33 . 

Using  the  properties  of  direction  cosines  that  the  dot  product  of  any  row  or  column  with  itself  is  1  the  raw  gravity 
measurements  can  be  normalized. 
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The  estimated  direction  cosines  at  the  next  measurement  are  found  by  propagating  the  direction  cosine  kinematics  in 
Eq.  (36). 
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Finally,  the  updated  direction  cosines  are  calculated  by  taking  a  combination  of  propagated  and  measured  values  and 


the  updated  Euler  angles  <p  and  0  can  be  found. 
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(j)[k  + 1]  =  sin  1  (-/13  [ k  + 1]) 


(42) 


(43) 


6  [k  + 1]  =  atan2  (V23  [k  + 1] ,  /33  [k  + 1] ) 


(44) 


Similarly,  for  \\i  Eqs.  (13),  (43),  and  (44)  can  be  used  to  calculate  a  measured  y/  in  while  an  estimated  1/7  is  found 
by  propagating  the  kinematics  in  (34). 


i/7  =  atan2  (-mw ,  mXM 


(45) 


1/ 7  =  (//■[&] +At 
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(46) 


Finally,  an  updated  vp  is  found  by  taking  a  combination  of  propagated  and  measured  values. 

yr  [k  + 1]  =  ay/  +  (1  -  a)y/ 


(47) 


C.  Comparison  of  Improved  Algorithms 
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To  demonstrate  the  performance  of  both  the  improved  algorithms,  measurement  data  was  generated  by 
adding  Gaussian  noise  with  a  standard  deviation  of  0.25  deg/s  and  a  bias  of  0.25  deg/s  to  the  angular  velocity  data 
shown  in  Fig.  1.  Results  from  improved  algorithms  are  shown  in  Figs.  7  and  8  with  errors  in  Figs.  9  and  10.  In  both 
cases,  the  angular  velocity  measurements  have  been  pre-filtered  with  a  first-order  filter  having  a  time  constant  of 
0.075  seconds.  For  Quest,  the  weights  appearing  in  Eq.  (28b)  were  selected  as  0.01,  0.07,  and  1.97  for  gravity, 
magnetic  vector,  and  the  pseudo-vector,  respectively.  For  the  gyro-compensated  algorithm,  a  was  selected  as  0.02.  It 
is  clear  from  both  Figs.  7  and  8  that  the  additional  information  from  the  gyros  has  reduced  the  errors  from  10  deg  to 
only  a  few  degrees.  Both  improved  algorithms  have  similar  error  bounds.  Flowever,  QUEST  with  a  pseudo-vector 
has  higher  frequency  content.  Either  algorithm  could  be  used,  however,  the  gyro-compensated  algorithm  is  quicker 
to  implement  and  is  the  algorithm  implement  in  flight  tests  discussed  later. 
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Figure  7.  QUEST  with  Pseudo- Vector  Estimates 
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Figure  10.  Gyro-Compensated  Euler  Angles  Errors 


V.  TEST  PLATFORM 

The  T-Rex  600  CF  helicopter  was  chosen  as  the  helicopter  platform.  This  platform  is  popular  among 
competition  remote  control  pilots  for  its  acrobatic  capability.  The  T-Rex  600  CF  is  a  full  size  radio  controlled 
helicopter  consisting  of  Carbon  Fiber  (CF)  and  CNC  aluminum  parts.  The  helicopter  measure  almost  4  feet  in 
length  The  T-Rex  600  uses  full  size  600mm  rotor  blades  and  is  powered  by  a  ballistic  combination  of  both  a  large 
brushless  motor  and  lithium  packs(in  series).  The  T-Rex  600  allows  for  the  ability  to  perform  3D  maneuvers.  The 
following  sections  are  a  few  design  features  of  the  T-Rex  600. 
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Figure  1:  T-Rex  600  Test  Helicopter 


The  T-Rex  rotor  head  is  made  from  a  combination  of  plastic  moldings  and  machined  aluminum  parts.  The  head 
uses  mixing  in  the  transmitter  to  drive  three  servos  linked  to  the  three  stationary  swash  plate  balls  at  120  degrees 
intervals.  This  moves  the  swash  plate  up  and  down  for  collective  pitch  and  tilts  it  for  cyclic  control  by  moving  the 
servos  in  combination.  Because  these  rotors  produce  small  damping  moments  in  comparison  to  larger  helicopters, 
the  design  features  stabilizer  bars  for  ease  of  handling. 

The  T-Rex  600  is  equipped  with  a  mechanical  Bell-Hiller  stabilizer  bar  that  affectively  applies  lagged  rate 
feedback  to  the  two  cyclic  control  channels.  This  system  may  be  regarded  as  a  secondary  rotor  attached  to  the  shaft 
below  the  main  rotor  by  an  unrestrained  teetering  hinge.  Aerodynamic  paddles  are  attached  to  the  ends  of  a  rod. 
Cyclic  pitch  and  roll  are  inputs  transmitted  to  the  stabilizer  bar.  But  unlike  the  main  rotor,  the  stabilizer  bar  has  no 
collective. 

VI.  CONTROL  ALGORITHMS 

The  small  rotorcraft  control  law  transforms  the  task  of  controlling  the  rotorcraft  over  the  flight  into  one  of 
controlling  the  servo  applications  for  roll,  pitch,  yaw,  and  altitude.  Information  about  the  desired  hover  point  is  used 
to  determine  when  to  correctly  adjust  the  swash  plate  and  the  heading.  In  hover,  the  desired  altitude  simply  becomes 
the  desired  height  above  the  ground.  Desired  roll  and  pitch  are  related  to  the  position  of  the  helicopter.  Figure  12 
shows  a  top  view  of  a  helicopter  with  a  desired  hover  location.  A  vector  r  describing  the  position  error  in  the 
helicopter  frame  is  defined  in  Eq.  (48).  Equations  (49)  and  (50)  relate  the  desired  roll  and  pitch  to  the  desired 
location  through  terms  proportional  to  the  position  and  velocity  errors. 
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Figure  12:  Desired  Roll  and  Pitch  Geometry 
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The  small  rotorcraft  control  algorithm  includes  three  components:  Position/Velocity  Filter,  Swash  Plate  Control, 
and  Heading  Lock.  Figure  13  illustrates  the  combined  flight  system  with  descriptions  and  block  diagrams  of  the 
individual  modules  provided  below.  Figure  14  demonstrates  the  T-Rex  600  helicopter’s  servo  location  with  respect 
to  the  swash  plate  and  tail  rotor.  This  representation  clarifies  the  output  channels  with  their  appropriate  servo. 
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Figure  13.  Autonomous  Small  Rotorcraft  Flight  Strategy 
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Figure  14.  Servo  Representation  and  Location  on  Swash  Plate  and  Tail  Rotor 


A.  Position/Velocity  Filter 

The  position/velocity  control  filter  illustrated  in  Figure  15  transforms  the  current  GPS  latitude  (latpojnt)  and 
longitude  (lonpoint)  points  of  the  rotorcraft  into  feet  and  subtracts  these  points  from  the  desired  hover  position  to 
calculate  the  current  lateral  and  longitudinal  position  error  vectors.  During  the  saturation  process,  a  scale  factor  is 
used  with  the  distance  vectors  to  preserve  the  magnitude  in  the  occurrence  of  large  position  errors.  The  derivatives 
of  the  distance  vectors  are  used  to  calculate  the  vehicle’s  current  velocity.  The  velocity  vector  is  saturated  with 
respect  to  Vmax-  Finally,  the  error  vectors  are  integrated  with  respect  to  the  previous  integral  error  and  featured  with 
a  forgetting  factor.  The  magnitude  of  this  integration  is  determined  to  provide  the  current  position  integral  error. 

The  outputs  of  the  position/velocity  filter  are  the  current  distance  vector,  the  current  vehicle  velocity,  and  the  current 
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integral  error.  The  control  system  is  updated  four  times  a  second.  (See  Appendix  for  description  of 
position/velocity  filter  block  elements.) 


Figure  15.  Position/Velocity  Control  Filter  Block  Diagram 

B.  Swash  Plate  Control 

The  output  of  position/velocity  filter  is  transferred  to  the  swash  plate  control  system  along  with  the  current 
vehicle  heading  and  the  desired  altitude.  The  distance,  velocity,  and  integral  errors  from  the  position/velocity  filter 
are  transformed  into  the  vehicle’s  body  frame  by  the  heading  angle,  yaw.  The  desired  roll  and  pitch  for  the  flight 
path  has  been  previously  described  in  Figure  12.  The  transformed  vectors  are  factored  with  their  appropriate  gain: 
proportional,  integral,  or  derivative  value.  The  values  in  the  x  direction  are  combined  together  to  provide  the  true 
roll  value,  (pTrue.  This  is  combined  with  the  sensor  roll  and  roll  rate  of  the  helicopter  plus  a  bias  and  their  appropriate 
gains.  The  values  in  the  y  direction  are  combined  together  to  provide  the  true  roll  value,  0True.  This  is  combined 
with  the  sensor  pitch  and  pitch  rate  of  the  helicopter  plus  a  bias  and  their  appropriate  gains.  The  outputs  then 
become  the  current  roll  and  pitch  control  of  the  helicopter  which  is  converted  to  servo  controls:  Channels  1  and  2. 

For  the  altitude  control,  the  current  altitude  is  inputted  using  a  barometric  altimeter  and  subtracted  with  the 
desired  altitude  for  the  flight.  The  current  altitude  error  is  saturated  with  respect  to  AltMAX.  The  current  altitude 
error  is  combined  with  current  velocity  and  current  altitude  integral  error  in  a  simple  proportion-integral-derivative 
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control  fashion  to  calculate  the  current  collective  pitch  of  the  vehicle  in  flight.  The  collective  output  is  then 
converted  to  servo  control:  Channel  6.  The  combined  roll,  pitch,  and  altitude  control  systems  seen  in  Figure  16 
move  the  swash  plate  of  the  vehicle  to  reach  the  desired  hover  height  and  location.  The  control  system  is  updated 
twelve  times  per  second.  See  Appendix  for  block  element  descriptions. 


lntalt(i-1) 

Figure  16:  Swash  Plate  Control  Block  Diagram 


C.  Heading  Lock  Control 

For  the  heading  lock  control  system  illustrated  in  Figure  17,  the  input  is  the  current  yaw  angle  which  is 
subtracted  from  the  desired  heading.  First,  the  yaw  error  is  saturated  with  pi  to  determine  the  correct  direction  of  the 
heading  angle.  Then,  the  yaw  error  is  saturated  with  respect  to  Yawerror  max-  The  current  yaw  error  is  combined 
with  the  current  integral  error  and  the  sensor  yaw  rate  in  a  proportional-integral-derivative  control  fashion  to 
determine  the  optimum  tail  position.  The  optimum  tail  position  is  saturated  with  respect  to  OptTaii  MAX.  The 
optimum  tail  position  is  converted  to  servo  control:  Channel  4.  The  heading  lock  control  system  is  updated  twelve 
times  per  second.  See  Appendix  for  block  element  descriptions. 
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Figure  17:  Heading  Lock  Control  Block  Diagram 


D.  Autopilot 

The  flight  control  system  hardware  include:  Multiple  microprocessors,  GPS,  three  axis  accelerometers, 
three  axis  magnetometer,  three  axis  gyroscope,  electronic  variometer,  airspeed  sensor  and  barometric  altimeter.  The 
autopilot  is  illustrated  in  Figure  18.  Figure  19  also  shows  the  autopilot  mounted  and  attached  to  the  T-Rex  600 
helicopter.  The  gyro-compensated  tilt  sensor  and  compass  was  selected  as  the  orientation  algorithm  due  to  its 
comparable  performance  with  the  optimal  QUEST  solution  and  its  simplicity. 


Figure  18:  Miniature  Autonomous  Rotorcraft  Autopilot 
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Figure  19:  Current  Autopilot  connected  to  T-Rex  600  Servos  and  Receiver 


E.  Graphical  User  Interface 

The  ability  to  communicate  wirelessly  with  the  autopilot  is  a  critical  component  in  the  autonomous  flight 
development.  This  achievement  is  made  possible  by  the  graphic  user  interface  (GUI)  seen  in  Figure  20.  The  GUI 
allows  the  user  to  view  real  time  sensor  data,  change  desired  wave  points  in  current  flight,  store  proportional- 
integral-derivative  gains,  calibrate  onboard  sensors  and  servo  parameters,  and  view  selected  variables  of  the  control 


algorithm. 


Figure  20:  Graphical  User  Interface 


27 


F.  Flight  Test  Results 


For  the  flight  test,  the  T-Rex  600  implemented  with  the  autopilot  was  taken  to  a  field  at  Dublin  Park,  in  Madison, 
Alabama.  The  helicopter  was  placed  stationary  on  the  field  until  a  reasonable  GPS  accuracy  was  reached.  While 
the  helicopter  was  stationary,  all  sensors  were  checked  and  calibrated  appropriately.  The  helicopter  was  then  placed 
in  the  center  of  the  soccer  field  and  that  specific  GPS  point  was  entered  as  the  desired  wave  point  into  the  graphical 
interface.  With  the  complete  control  algorithm  programmed  and  all  servos  attached  to  the  autopilot,  the  helicopter 
was  taken  to  an  altitude  of  5  ft  manually  and  switched  to  autonomous  mode.  A  camera  was  mounted  to  the  bottom 
of  the  landing  gear  to  track  the  desired  point  in  the  center  of  the  soccer  field,  which  was  marked  by  a  2ft  x  2ft  x  2ft 
black  case.  Gains  for  desired  roll  and  pitch  KP,  KD,  K,  were  selected  as  0.00017,  0.00033,  and  0.0006  deg, 
respectively.  The  T-Rex  600  was  initialized  at  x  =  8ft  west  and  y  =  60ft  north,  z  =  0m,  <p  =  0.6  deg,  0  =  2deg  and  \| r 
=  -142  deg.  The  desired  position  is  x  =  y  =0ft,  z  =  30ft  and  the  desired  orientation  is  0  =  (p  =  0  deg  and  \|/  =  -142 
deg.  Figure  21  shows  the  real  time  flight  data  for  altitude.  Sensor  noise  that  is  amplified  by  the  PID  gains  causes 
the  helicopter  to  oscillate  about  the  desired  height  of  30ft  by  ±10ft.  Figure  22  displays  the  helicopter  speed  which 
remains  about  3  ft/s  for  the  duration  of  the  flight.  In  Fig.  23,  the  orientation  of  the  rotorcraft  is  shown  to  track  the 
desired  angles  appropriately.  Figure  24  provides  the  real  time  flight  trajectory  for  the  entire  flight.  The  red  circle 
provided  in  the  graph  displays  20ft  diameter  in  which  the  helicopter  is  trying  to  hover.  Finally,  Figure  25  is  a  real 
time  shot  of  the  helicopter  in  flight  tracking  the  desired  location  marked  by  the  2ft  x  2ft  x  2ft  black  case. 
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Figure  21.  Real  Time  Altitude  Data  Tracking  Desired  30ft 
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Figure  23.  Real  Time  Flight  Orientation 


E-W  (FT) 

Figure  24.  Real  Time  Flight  Trajectory  within  20ft  Circle 
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Figure  25.  Real  Time  Flight  Video  Containing  Bottom  View  of  Tracking  Desired  Flover  Location 
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Appendix 


Block  Element 

Description 

MAG 

Dist(i)  =  sqrt(dion(i)*dion(i)  +  diat(i)*diat(i)) 

< 

BAT 

A 

DistMAX 

/•••• 

SF  =  125/Dist(i) 

dion(i)  =  dion(i)  *  SF  ,  if  Dist(i)  >  125  ft 

diat(i)  =  diat(i)  *  SF 

Dist(i)  =  125 

dion(i)  =  0.0  ,  if  Dist(i)  <  1.0  ft 

dUO  =  0.0 

SF=  1.0-  1.0/Dist(i) 

dion(i)  =  dion(i)  *  SF  ,  if  Dist(i)  <125  ft 

diat(i)  =  diat(i)  *  SF 

Sf  =  60/Dist(i),  if  Dist(i)  >  60.0  ft 

-DistM/s 

w 

X 

Eqn.1 

dlon  (0  =  2.0*(  dlon(i)  -dlon(i-l)  +  0.5*  dlon  (/)  ; 

Eqn.2 

dlat(i)  =  2.0*(  d,at(i)  -dlat(i-l)  +  0.5*c/to(0  ; 

MAG 

V(i)  =  sqrt(  d/on  (/)  *  dlon  (/)  +dlat  (/)  *  dlat  (/)  ); 

SAT 

i 

Vmax 
k, . 

dhn(i)  =  dlon (/)  *27.0/V(i) 

d,at  (0  =  dlat  ( i )  *27.0/V(i) ,  if  V(i)  >  27.0  ft/s 

■Vmax 

w 

Eqn.3 

intion(i)  =  intion(i)  *Ff  +  dt*  intlon(i)  *SF; 

Eqn.4 

intiat(i)  =  intlat(i)  *Ff  +  dt*  intlat(i)  *SF; 

MAG 

intcrr(i)  =  sqrt(int,on(i)  *  intlon(i)  +  intlat(i)  *  int,at(i)); 

sinyaw  =  sin(\|/(i)); 

cosyaw  =  cos(\|/  (i)); 

rx=  cosyaw*  dia,(i)  +  sinyaw*  dion(i); 

ry  =  -sinyaw*  diat(i)  +  cosyaw*  dlon(i); 

rx  =  cosyaw*  dlat  (/)  +  sinyaw*  dlon  (/)  ; 

rv  =  -sinyaw*  dIat  (/)  +  cosyaw*  dlon  (/) ; 

int,  =  cosyaw*  intlat(i)  +  sinyaw*  intlon(i); 
intv  =  -sinyaw*  intiat(i)  +  cosyaw*  intlon(i); 

Desired 

Roll/Pitch 

Geometry 
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